package controlSoftware.DataProcess;

import java.io.IOException;
import java.text.DecimalFormat;
import java.util.Queue;
import java.util.logging.FileHandler;
import java.util.logging.Logger;

import controlSoftware.GUI.mainUI;
import controlSoftware.util.DataFormatter;
import controlSoftware.view.viewController;

public class DataProcess extends Thread {

	/** variables **/
	private int id;
	private String threadName;

	private int counter = 0;
	private int initNum = 20;
	private SensorData[] offset_array = new SensorData[initNum];
	private Device deviceObj;
	private long timeElapsed;
	static final long DEFAULTTIMEINTERVAL = 120;
	
	/** Data manipulation **/
	private Queue<SensorData> rawDataQueue; // Queue with IMU data
	
	public SensorData currentData;
	public SensorData prevData = SysModel.zeroPt;

	public void setPrevData(SensorData data) {
		this.prevData = data;
	}
	
	public SensorData getPrevData() {
		return this.prevData;
	}

	public SensorData getNextFromQueue() {
		// returns head of queue, or null if empty
		return rawDataQueue.poll();
	}
	
	/** Velocity Calculation **/
	Velocity prevVelocity = SysModel.initVelocity;
	public Velocity getPrevVelocity() {
		return prevVelocity;
	}

	public void setPrevVelocity(Velocity prevVelocity) {
		this.prevVelocity = prevVelocity;
	}

	/** Coordinate Calculation **/
	private Coordinate prevCoordinate = SysModel.initCoordinate;
	
	public Coordinate getPrevCoordinate() {
		return prevCoordinate;
	}
	public void setPrevCoordinate(Coordinate prevCoordinate) {
		this.prevCoordinate = prevCoordinate;
	}	
	

	/** View Controller **/
	public viewController viewControl;
	public viewController getViewController()
	{
		return this.viewControl;
	}

	/**
	 * Constructor for DataProcess thread
	 */
	public DataProcess(int id, String threadName, Queue<SensorData> cache, Device deviceObj) {
		this.id = id;
		this.threadName = threadName;
		this.rawDataQueue = cache;
		this.prevVelocity = SysModel.initVelocity;
		this.prevCoordinate = SysModel.initCoordinate;
		this.deviceObj = deviceObj;
		
		//this.userFilter = new KalmanFilter();
		this.viewControl = new viewController(deviceObj);

	}

	public String getThreadName() {
		return threadName;
	}


	/**
	 * Thread execution
	 */
	@Override
	public void run() {
		while (!this.isInterrupted()) 
		{	// while the thread doesn't get interrupted
			
			// get raw data from the queue
			if (!rawDataQueue.isEmpty()) {
				currentData = this.getNextFromQueue();
				//System.out.println("dp--->" + currentData.accel_x+","+currentData.accel_y+","+currentData.accel_z);

			} else {
				// don't do anything
				currentData = null; 
			}

			if (currentData != null) {
				// Calculate the offset in IMU data at zero acceleration state
				if (SysModel.isInitState) {
					offset_array[counter]=currentData; 
					counter ++; 
					
					// obtain a defined number of data and obtain average
					if (counter == initNum) { 
						calculateOffset(offset_array);
						SysModel.isInitState = false; 
					} 
				}
				else
				{
					// convert raw data to acceleration values
					currentData.processRaw(prevData.getAccel_x(), prevData.getAccel_y());
	
					timeElapsed = currentData.getTimeStamp() - prevData.getTimeStamp();
					if(timeElapsed > 500){
						timeElapsed = SysModel.sampleTime;
					}
					
					// calculate current velocity 
					currentData.getVelocity(prevVelocity, timeElapsed);
	
					// calculate displacement
					currentData.getDisplacement(prevVelocity, timeElapsed);
					
					// calculate new coordinate
					currentData.getCoordinate(prevCoordinate);
	
					// pass on the data to view handler to handle
					viewControl.updateView(currentData, prevCoordinate);
	
					// stores current information for next iteration
					prevCoordinate = currentData.getCurrLoc();
					prevVelocity = currentData.getCurrVel();
					prevData = currentData;

					DecimalFormat formatter = new DecimalFormat("##.00");
					
					mainUI.statusBar.setText("DataProcess " + id + ": " +
							currentData.getTimeStamp() + ","
							+ formatter.format(currentData.getCurrLoc().getX()) + ","
							+ formatter.format(currentData.getCurrLoc().getY())
							);
				}
			}
		}
	}

	/**
	 * Calculates the average of the values in the array
	 * @param array
	 */
	private void calculateOffset(SensorData[] array) {
		double x = 0;
		double y = 0;
		int i;

		for (i = 0; i < initNum; i++) {
			x = x + array[i].getAccel_x();
			y = y + array[i].getAccel_y();
		}

		SysModel.ax_offset = x / initNum;
		SysModel.ay_offset = y / initNum;

	}

}